/****************************************************************************
 *
 *  Heterogeneous 10PWM passthrough module (systemcmds copy)
 *
 *  This file is a copy of src/modules/hetero_passthrough/hetero_passthrough.cpp
 *  placed under src/systemcmds so the binary is packaged into ROMFS /bin for
 *  quick testing. Behavior and safety checks are identical to the module
 *  version: it subscribes to manual_control_setpoint and publishes actuator_motors
 *  when the vehicle is armed.
 */

#include <px4_platform_common/module.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/module_params.h>

#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_angular_velocity.h>

#include <lib/mathlib/mathlib.h>
#include <matrix/math.hpp>

using namespace time_literals;

class HeteroPassthrough : public ModuleBase<HeteroPassthrough>, public px4::ScheduledWorkItem
{
public:
    HeteroPassthrough() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) {}

    virtual ~HeteroPassthrough() { ScheduleClear(); }

    static int task_spawn(int argc, char *argv[])
    {
        HeteroPassthrough *instance = new HeteroPassthrough();

        if (!instance) {
            PX4_ERR("alloc failed");
            return PX4_ERROR;
        }

        _object.store(instance);

        if (instance->init()) {
            return PX4_OK;
        }

        delete instance;
        _object.store(nullptr);
        return PX4_ERROR;
    }

    bool init()
    {
        // schedule at 50 Hz
        ScheduleOnInterval(20_ms);
        return true;
    }

    void Run() override
    {
        if (should_exit()) {
            ScheduleClear();
            return;
        }

        // check manual control
        // use copy() to always get the latest sample (even if not "new" since
        // last Run()). This ensures we re-publish zeros when sticks return to
        // center instead of early-returning and leaving previous actuator values
        // active downstream.
        manual_control_setpoint_s manual{};
        _manual_sub.copy(&manual);

        // check armed status
        vehicle_status_s vstatus{};
        _vehicle_status_sub.copy(&vstatus);

        // only publish when armed for safety
        if (vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
            return;
        }

        actuator_motors_s motors{};
        motors.timestamp = hrt_absolute_time();
        motors.timestamp_sample = manual.timestamp_sample;

        // initialize as 0 to ensure unused channels are explicitly zeroed
        // (avoids leaving stale/NaN values that downstream mixers/drivers
        // might interpret unexpectedly)
        for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
            motors.control[i] = 0.0f;
        }

        const float deadzone = 0.05f;
        // small absolute threshold to force values very close to zero to 0.0f
        // helps avoid low-amplitude jitter causing motors to spin slowly.
        const float zero_threshold = 0.02f;

        const float throttle = manual.throttle; // [-1,1], mid=0 (美国手左摇杆上下)
        const float yaw = manual.yaw;           // [-1,1], mid=0 (美国手左摇杆左右)
        const float pitch = manual.pitch;       // [-1,1], mid=0 (美国手右摇杆上下)
        const float roll = manual.roll;         // [-1,1], mid=0 (美国手右摇杆左右)

// Yaw控制 - 按照多旋翼标准控制方式实现
        float yaw_control = 0.0f;
        const float yaw_deadzone = 0.05f; // con自定义死区，可独立于其他控制自定义死区，可独立于其他控制

        // 获取当前时间于PID计算
        hrt_abstime current_time =thrt_absolute_time();nt_time = hrt_absolute_time();
        float d/ 默认时间步1长//默认时间步长

        // 计算时间差
        ii (_f(s_i0ime>) {
            dt = (current_time - _last_time)    e6f; // 转换为秒, 0.001f, 0.1f);  // 限制时间步长范围
            d= math::contain(d,01, 0.1f)  // 限制时间步长范围_last_time = current_time;
}
        _速aticurrent_im

      读取姿态和角速度数据
        vehiv_e_tttitudu_t {tt}tude{}vehicle_angular_velocity_s angular_velocity{};
vehcl_angularvelcitys agulr_veocity};

        bool has_attitude = _vehicle_attitude_sub.copy(&attitude);
        bool has_angular_velocity = _vehicle_angular_velocity_sub.copy(&angular_velocity);
        bool has_valid_sensors = has_attitude && has_angular_velocity && dt > 0.001f;

        // 关键决策：检查遥控器输入状态
        bool has_manual_yaw_input = (fabsf(yaw) > yaw_deadzone);

        // 自旋控制核心逻辑（按照多旋翼标准实现）
        if (has_manual_yaw_input) {
            // 🔑 互斥性原则：有手动输入时关闭姿态保持
            // 遥控器直接控制角速度比例
            yaw_control = yaw;
            // 重置积分器，避免切换回来时产生剧烈反应
            _yaw_integral = 0.0f;
            // 重置目标角度
            _last_yaw = 0.0f;

        } else if (has_valid_sensors) {
            // 🔑 优先级原则：无手动输入时启用PID姿态保持
            // 平滑切换：重新建立姿态保持目标

            // 从四元数提取当前yaw角度
            matrix::Eulerf euler_angles(matrix::Quatf(attitude.q));
            float current_yaw = euler_angles.psi();

            // 重置目标角度（首次进入姿态保持模式）
            if (fabsf(_last_yaw) < 0.001f) {
                _last_yaw = current_yaw;  // 设置新的姿态保持目标
            }

            // 🔑 姿态保持模式：PID控制
            float target_yaw = _last_yaw;

            // 计算yaw角度误差（处理角度环绕）
            float yaw_error = matrix::wrap_pi(target_yaw - current_yaw);

            // 获取当前yaw角速度
            float current_yaw_rate = angular_velocity.xyz[2]; // z轴角速度

            // 标准PID控制实现
            const float kp = 0.0002f;  // 增大比例增益
            const float ki = 0.0f;  // 减小积分增益
            const float kd = 0.0f;  // 增大微分增益（增强阻尼）
            const float integral_limit = 0.3f;  // 降低积分限幅
            const float output_limit = 0.5f;    // 降低输出限幅

            // 比例项
            float p_term = kp * yaw_error;

            // 🔑 平滑切换原则：积分项限幅保护
            _yaw_integral += ki * yaw_error * dt;
            _yaw_integral = math::constrain(_yaw_integral, -integral_limit, integral_limit);

            // 微分项（阻尼）
            float d_term = -kd * current_yaw_rate;

            // PID输出合成
            float yaw_correction = p_term + _yaw_integral + d_term;
            yaw_correction = math::constrain(yaw_correction, -output_limit, output_limit);

            yaw_control = yaw_correction;

            // 调试信息（节省频率）
            if ((int)(current_time / 1000000) % 10 == 0) {
                PX4_INFO("Yaw Hold: 目标=%.2f 当前=%.2f 误差=%.3f 修正=%.3f PID=%.3f,%.3f,%.3f",
                        (double)target_yaw, (double)current_yaw, (double)yaw_error, (double)yaw_control,
                        (double)p_term, (double)_yaw_integral, (double)d_term);
            }
        }

        // 存储控制结果
        float final_yaw_control = yaw_control;

        // 垂直控制 - 油门
        // 油门上升：电机1-2转动
        if (throttle > deadzone) {
            float val = math::constrain(throttle, 0.0f, 1.0f);
            motors.control[0] = val; // 电机1
            motors.control[1] = val; // 电机2
        }
        // 油门下降：电机3-4转动
        else if (throttle < -deadzone) {
            float val = math::constrain(-throttle, 0.0f, 1.0f);
            motors.control[2] = val; // 电机3
            motors.control[3] = val; // 电机4
        }

        // 🔑 多旋翼标准自旋控制：手动输入与姿态保持模式切换
        // 左摇杆向左：电机7-8旋转
        if (final_yaw_control < -yaw_deadzone) {
            float val = math::constrain(-final_yaw_control, 0.0f, 1.0f);
            motors.control[6] = val; // 电机7
            motors.control[7] = val; // 电机8
        }
        // 左摇杆向右：电机5-6旋转
        else if (final_yaw_control > yaw_deadzone) {
            float val = math::constrain(final_yaw_control, 0.0f, 1.0f);
            motors.control[4] = val; // 电机5
            motors.control[5] = val; // 电机6
        }

        // 水平移动控制 - 右摇杆
        // 右摇杆向上（前进）：电机6和8旋转
        if (pitch > deadzone) {
            float val = math::constrain(pitch, 0.0f, 1.0f);
            motors.control[5] += val; // 电机6
            motors.control[7] += val; // 电机8
        }
        // 右摇杆向下（后退）：电机5和7旋转
        else if (pitch < -deadzone) {
            float val = math::constrain(-pitch, 0.0f, 1.0f);
            motors.control[4] += val; // 电机5
            motors.control[6] += val; // 电机7
        }

        // 右摇杆向左（左移）：电机5和8旋转
        if (roll < -deadzone) {
            float val = math::constrain(-roll, 0.0f, 1.0f);
            motors.control[4] += val; // 电机5
            motors.control[7] += val; // 电机8
        }
        // 右摇杆向右（右移）：电机6和7旋转
        else if (roll > deadzone) {
            float val = math::constrain(roll, 0.0f, 1.0f);
            motors.control[5] += val; // 电机6
            motors.control[6] += val; // 电机7
        }

        // ===================================
        // 🚀 9号电机 + 13/14号舵机扩展接口
        // ===================================

        // 9号电机：直接与遥控器油门相关（测试用）
        // 提供纯线性映射，便于单独测试
        float motor_9_output = 0.0f;

        // 13号舵机：与遥控器横滚相关（测试用）
        float servo_13_output = 0.0f;

        // 14号舵机：与抚养轴（pitch）相关（测试用）
        float servo_14_output = 0.0f;

        // 📊 测试模式配置（参数控制）
        const bool test_mode_enabled = true;  // 测试模式开关
        const float test_response_gain = 0.8f; // 测试响应强度（防止过冲）

        if (test_mode_enabled) {
            // 🔧 9号电机：直接映射油门信号（-1,1）→（0,1）
            motor_9_output = (throttle + 1.0f) * 0.5f * test_response_gain;
            motor_9_output = math::constrain(motor_9_output, 0.0f, 1.0f);

            // 🔧 13号舵机：横滚信号（-1,1）→（0.1,0.9）中位保护
            servo_13_output = (roll + 1.0f) * 0.4f + 0.1f;
            servo_13_output = math::constrain(servo_13_output, 0.0f, 1.0f);

            // 🔧 14号舵机：俯仰信号（-1,1）→（0.1,0.9）中位保护
            servo_14_output = (pitch + 1.0f) * 0.4f + 0.1f;
            servo_14_output = math::constrain(servo_14_output, 0.0f, 1.0f);

            // 调试用输出（每5秒一次，避免日志过多）
            static int debug_counter_servo = 0;
            if (++debug_counter_servo % 250 == 0) {
                PX4_INFO("拓展测试: Motor9=%.2f Servo13=%.2f Servo14=%.2f",
                        (double)motor_9_output, (double)servo_13_output, (double)servo_14_output);
                debug_counter_servo = 0;
            }
        }

        // 🔗 映射到扩展执行器通道
        // 假设使用control[8], control[12], control[13]作为扩展接口
        motors.control[8] = motor_9_output;    // 9号电机通道
        motors.control[12] = servo_13_output;  // 13号舵机通道
        motors.control[13] = servo_14_output;  // 14号舵机通道

        // 约束所有电机输出在[0,1]范围内（包含扩展通道）
        for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
            motors.control[i] = math::constrain(motors.control[i], 0.0f, 1.0f);
        }

        // Debug: print actuator_motors controls to console for troubleshooting
        PX4_INFO("hetero_passthrough motors: %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f",
                 (double)motors.control[0], (double)motors.control[1], (double)motors.control[2], (double)motors.control[3],
                 (double)motors.control[4], (double)motors.control[5], (double)motors.control[6], (double)motors.control[7]);

        // clamp small values to zero to avoid micro-output
        for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
            if (fabsf(motors.control[i]) < zero_threshold) {
                motors.control[i] = 0.0f;
            }
        }

        _motors_pub.publish(motors);
    }

    static int custom_command(int argc, char *argv[])
    {
        return print_usage("unknown command");
    }

    static int print_usage(const char *reason = nullptr)
    {
        if (reason) {
            PX4_WARN("%s", reason);
        }

    PRINT_MODULE_DESCRIPTION(
        R"DESCR_STR(
### Description
Heterogeneous 10PWM passthrough: maps manual stick inputs to actuator_motors directly.

Use with caution: bypasses attitude control and control allocator.
)DESCR_STR"
    );

        PRINT_MODULE_USAGE_NAME("hetero_passthrough", "controller");
        PRINT_MODULE_USAGE_COMMAND("start");
        PRINT_MODULE_USAGE_COMMAND("stop");
        PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

        return 0;
    }

private:
    uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)};
    uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
    uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
    uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
    uORB::Publication<actuator_motors_s> _motors_pub{ORB_ID(actuator_motors)};

    // PID控制变量
    float _yaw_integral{0.0f};
    float _last_yaw{0.0f};
    hrt_abstime _last_time{0};
};

extern "C" __EXPORT int hetero_passthrough_main(int argc, char *argv[])
{
    return HeteroPassthrough::main(argc, argv);
}
